#include "distance.h"

using namespace std;
using namespace cv;

void Distance::setCameraParameter(const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, const double squareLength, const double scale)
{
	this->cameraMatrix = cameraMatrix.clone();
	this->distCoeffs = distCoeffs.clone();
	this->squareLength = squareLength;
	this->scale = scale;
}

void Distance::getObjectPoints(std::vector<cv::Point3f>& objectPoints)
{
	objectPoints.resize(4);
	objectPoints[0] = { -squareLength / 2, squareLength / 2, 0 };
	objectPoints[1] = { squareLength / 2, squareLength / 2, 0 };
	objectPoints[2] = { squareLength / 2, -squareLength / 2, 0 };
	objectPoints[3] = { -squareLength / 2, -squareLength / 2, 0 };
}

double Distance::calculate(const cv::Mat& image)
{
	if (cameraMatrix.empty() || distCoeffs.empty() || squareLength <= 0 || scale <= 0) {
		return -1;
	}

	vector<Point3f> objectPoints;
	getObjectPoints(objectPoints);

	vector<Point2f> imagePoints;
	getImagePoints(image, imagePoints);

	if (objectPoints.empty() || imagePoints.empty() || objectPoints.size() != imagePoints.size()) {
		return -1;
	}

	cv::Mat rvec, tvec;
	bool retval = solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs,
		rvec, tvec, false, SOLVEPNP_IPPE_SQUARE);
	if (!retval) {
		return -1;
	}

	cv::Mat rotateMatrix;
	Rodrigues(rvec, rotateMatrix);
	cv::Mat transMatrix = -rotateMatrix.inv() * tvec;

	return transMatrix.at<double>(2) * scale;
}

